#include "main.h"
#include "tim.h"
#include "encoder.h"

extern int motorSpeed[4];

void encoderInit(void)
{
    HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_ALL);
    
    HAL_TIM_Base_Start_IT(&htim6);
}

void getSpeed(void)
{
    motorSpeed[0] = (short)__HAL_TIM_GET_COUNTER(&htim2);
    motorSpeed[1] = (short)__HAL_TIM_GET_COUNTER(&htim3);
    motorSpeed[2] = -(short)__HAL_TIM_GET_COUNTER(&htim4);
    motorSpeed[3] = -(short)__HAL_TIM_GET_COUNTER(&htim5);
    
    __HAL_TIM_SET_COUNTER(&htim2,0);
    __HAL_TIM_SET_COUNTER(&htim3,0);
    __HAL_TIM_SET_COUNTER(&htim4,0);
    __HAL_TIM_SET_COUNTER(&htim5,0);
}
